/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.InertialReference;
import edu.wpi.first.wpilibj.templates.subsystems.DriveSubsystem;

/**
 *
 * @author Jarrod Risley
 * 
 * This class should only be used for AutonMode, as it commands the 
 * left and right subsystems to move based upon a voltage number. It might 
 * be of use in other applications, but for now just let it be.
 */
public class InchForward extends CommandBase {

    double commandedPercent;
    boolean commandSent;
    // needed classes
    DriveSubsystem.DriveMode leftPrevMode = DriveSubsystem.DriveMode.kUninitialized;
    DriveSubsystem.DriveMode rightPrevMode = DriveSubsystem.DriveMode.kUninitialized;

    public InchForward(double joystickValue) {
        requires(leftDriveSubsystem);
        requires(rightDriveSubsystem);

        commandedPercent = -joystickValue;

        System.out.println("Autonomous Drive Constructed: " + joystickValue + " Joystick Value Things");
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        // get the old drive modes
        leftPrevMode = leftDriveSubsystem.getDriveMode();
        rightPrevMode = rightDriveSubsystem.getDriveMode();

        // set to speed mode on both sides
        leftDriveSubsystem.setSpeedMode();
        rightDriveSubsystem.setSpeedMode();
        commandSent = false;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        if (!commandSent) {
            rightDriveSubsystem.drive(commandedPercent);
            leftDriveSubsystem.drive(commandedPercent);
            commandSent = true;
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (commandSent
                && rightDriveSubsystem.isMotionComplete()
                && leftDriveSubsystem.isMotionComplete());
    }

    // Called once after isFinished returns true
    protected void end() {
        if (DriveSubsystem.DriveMode.kSpeedControl == leftPrevMode) {
            leftDriveSubsystem.setSpeedMode();
        } else if (DriveSubsystem.DriveMode.kDistanceControl == leftPrevMode) {
            leftDriveSubsystem.setDistanceMode();
        }

        if (DriveSubsystem.DriveMode.kSpeedControl == rightPrevMode) {
            rightDriveSubsystem.setSpeedMode();
        } else if (DriveSubsystem.DriveMode.kDistanceControl == rightPrevMode) {
            rightDriveSubsystem.setDistanceMode();
        }
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    }
}
